clc;
clear;
close all;

h=0.0001;
tf=5;q0=0;qf=90*pi/180;
a=50*pi/180;%加速度值
ta=tf/2-(sqrt(a*a*tf*tf-4*a*(qf-q0))/(2*a));%加减速时间

L=0.1; 
m=0.5;
g=9.8;
gra_bool=1;
b=0.02;

i=1;
for t=0:h:tf
    [q,dq,ddq]=position_S(tf,q0,qf,a,ta,t);
    ddq_expected=ddq;
    [torque_expected(i),torque_inter(i),torque_linear(i)]=inverse_dynamic(L,m,g,q,dq,ddq_expected,b,gra_bool);
%     torque_expected(i) = q;
%     torque_inter(i) = dq;
%     torque_linear(i) = ddq;
    i=i+1;
end

t=0:h:tf;

figure(1)
plot(t,torque_expected)
title('理想总驱动力矩')
grid on
figure(2)
plot(t,torque_inter)
title('干扰力矩')
grid on
figure(3)
plot(t,torque_linear)
title('等效线性力矩')
grid on